#ifndef MANIPULATE_H
#define MANIPULATE_H

#define BOX_FRICTION 0.85f
#define GROUND_FRICTION 0.75f
#define FINGER_FRICTION 1.5f
#define BOX_DENSITY 1300.0f;
#define FINGER_DENSITY 1069.0f;
#define BOX_SIZE 0.5f
#define FINGER_SIZE 0.05f
#define MOUSE_JOINT_FREQUENCY 15.0f
#define MOUSE_JOINT_DAMPING_RATIO 1.0f
#define SPACE 0.001f
#define FORCE_SCALE 0.00001f
#define POSITION_SCALE 0.5f
#define GROUND_POSITION b2Vec2(0.0f, 0.0f)
#define FINGER_POSITION b2Vec2(-1.000000, 1.850000)

#include <vector>
#include <cstdlib>
#include <sstream>
#include "Finger.h"
#include "Box.h"
#include "Ground.h"
#include "Adaptation.h"
#include "RecordInfo.h"
#include "Phase.h"
#include "ActualInfo.h"
#include "Noise.h"

class Manipulate : public Test
{
public:
	enum
	{
		e_boxDensityBit	= 0x0001,
		e_boxSizeBit = 0x0002,
		e_boxFrictionBit = 0x0004,
		e_fingerFrictionBit = 0x0008,
		e_groundFrictionBit = 0x0010,
		e_frequencyBit = 0x0020,
		e_dampingRatioBit = 0x0040,
		e_boxShiftingUpBit = 0x0080,
		e_boxShiftingDownBit = 0x0100,
		e_boxShiftingLeftBit = 0x0200,
		e_boxShiftingRightBit = 0x0400
	};

	enum
	{
		e_writeForce = 0x0001,
		e_writePosition = 0x0002,
		e_writeVelocity = 0x0004
	};

	class QueryCallback : public b2QueryCallback
	{
	public:
		QueryCallback(const b2Vec2& point)
		{
			m_point = point;
			m_fixture = NULL;
		}

		bool ReportFixture(b2Fixture* fixture)
		{
			b2Body* body = fixture->GetBody();
			if (body->GetType() == b2_dynamicBody)
			{
				bool inside = fixture->TestPoint(m_point);
				if (inside)
				{
					m_fixture = fixture;
					return false;
				}
			}
			return true;
		}

		b2Vec2 m_point;
		b2Fixture* m_fixture;
	};

	Manipulate()
	{		
		float32 boxFriction = BOX_FRICTION;
		float32 groundFriction = GROUND_FRICTION;
		float32 fingerFriction = FINGER_FRICTION;
		float32 boxDensity = BOX_DENSITY;
		float32 boxSize = BOX_SIZE;
		float32 fingerDensity = FINGER_DENSITY;
		float32 fingerSize = FINGER_SIZE;
		b2Vec2 groundPosition = GROUND_POSITION;
		b2Vec2 fingerPosition = FINGER_POSITION;

		m_trajectoryInput = NULL;
		m_trajectoryOutput = NULL;
		m_dataFile = NULL;
		m_actualFile = NULL;
		m_noise = NULL;
		m_replayIterations = -1;

		m_mouseJointFrequency = MOUSE_JOINT_FREQUENCY;
		m_mouseJointDampingRatio = MOUSE_JOINT_DAMPING_RATIO;

		m_adaptation = NULL;
		m_box = new Box(m_world, b2Vec2(-1.5f + boxSize, groundPosition.y + boxSize), boxSize, boxDensity, boxFriction);
		m_ground = new Ground(m_world, groundPosition, groundFriction);
		m_finger = new Finger(m_world, fingerPosition, fingerSize, fingerDensity, fingerFriction);

		m_externalForceCollection.reserve(3200 * 5);
		m_fingerPositionCollection.reserve(3200 * 5);
		m_fingerVelocityCollection.reserve(3200 * 5);
		m_recordInfos.reserve(3200 * 5);
		m_actualInfos.reserve(3200 * 5);
	}

	~Manipulate()
	{
		delete m_finger;
		delete m_box;
		delete m_ground;
		delete m_adaptation;
	}

	static Test* Create()
	{
		return new Manipulate;
	}

	void MouseDown(const b2Vec2& p)
	{
		if(!m_mouseJoint)
		{
			b2AABB aabb;
			b2Vec2 d;
			d.Set(0.001f, 0.001f);
			aabb.lowerBound = p - d;
			aabb.upperBound = p + d;

			QueryCallback callback(p);
			m_world->QueryAABB(&callback, aabb);

			if (callback.m_fixture)
			{
				b2Body* body = callback.m_fixture->GetBody();
				if(body == m_finger->GetFingerBody())
				{
					b2Vec2 fingerPosition = m_finger->GetFingerPosition();
					b2MouseJointDef md;
					b2BodyDef bodyDef;
					b2Body* emptyBody = m_world->CreateBody(&bodyDef);
					md.bodyA = emptyBody;
					md.bodyB = m_finger->GetFingerBody();
					md.target = fingerPosition;
					md.maxForce = 10000.0f * body->GetMass();
					md.dampingRatio = m_mouseJointDampingRatio;
					md.frequencyHz = m_mouseJointFrequency;
					m_finger->SetPullPointPosition(fingerPosition);
					m_mouseWorld = fingerPosition;
					m_mouseJoint = (b2MouseJoint*)m_world->CreateJoint(&md);
					m_finger->GetFingerBody()->SetAwake(true);
				}
			}
		}
	}

	void MouseMove(const b2Vec2& p)
	{
		if (m_mouseJoint)
			m_mouseJoint->SetTarget(p);
		m_finger->SetPullPointPosition(p);
	}

	void MouseUp(const b2Vec2& p)
	{
		if(m_mouseJoint)
		{
			m_world->DestroyJoint(m_mouseJoint);
			m_mouseJoint = NULL;
		}
	}

	void BeginContact(b2Contact* contact)
	{
		b2Fixture* fixtureA = contact->GetFixtureA();
		b2Fixture* fixtureB = contact->GetFixtureB();

		if (fixtureA == m_finger->GetFingerFixture())
		{
			void* userData = fixtureB->GetBody()->GetUserData();
			if (userData)
			{
				bool* touching = (bool*)userData;
				*touching = true;
			}
		}

		if (fixtureB == m_finger->GetFingerFixture())
		{
			void* userData = fixtureA->GetBody()->GetUserData();
			if (userData)
			{
				bool* touching = (bool*)userData;
				*touching = true;
			}
		}
	}

	//// Implement contact listener.
	void EndContact(b2Contact* contact)
	{
		b2Fixture* fixtureA = contact->GetFixtureA();
		b2Fixture* fixtureB = contact->GetFixtureB();

		if (fixtureA == m_finger->GetFingerFixture())
		{
			void* userData = fixtureB->GetBody()->GetUserData();
			if (userData)
			{
				bool* touching = (bool*)userData;
				*touching = false;
			}
		}

		if (fixtureB == m_finger->GetFingerFixture())
		{
			void* userData = fixtureA->GetBody()->GetUserData();
			if (userData)
			{
				bool* touching = (bool*)userData;
				*touching = false;
			}
		}
	}

	void Step(Settings* settings)
	{
		uint32 flags = 0;
		flags += settings->enableBoxDensityNoise * e_boxDensityBit;
		flags += settings->enableBoxSizeNoise * e_boxSizeBit;
		flags += settings->enableBoxFrictionNoise * e_boxFrictionBit;
		flags += settings->enableFingerFrictionNoise * e_fingerFrictionBit;
		flags += settings->enableGroundFrictionNoise * e_groundFrictionBit;
		flags += settings->enableMouseJointFrequencyNoise * e_frequencyBit;
		flags += settings->enableMouseJointDampingRatioNoise * e_dampingRatioBit;
		flags += settings->enableBoxShiftingUpNoise * e_boxShiftingUpBit;
		flags += settings->enableBoxShiftingDownNoise * e_boxShiftingDownBit;
		flags += settings->enableBoxShiftingLeftNoise * e_boxShiftingLeftBit;
		flags += settings->enableBoxShiftingRightNoise * e_boxShiftingRightBit;
		m_noiseFlag = flags;

		b2Vec2 fingerPosition = m_finger->GetFingerPosition();
		if(!m_mouseJoint)
		{
			m_finger->SetPullPointPosition(fingerPosition);
			m_finger->SetFingerReactionForce(b2Vec2(0.0f, 0.0f));
		}
		else
		{
			m_finger->SetFingerReactionForce(m_mouseJoint->GetReactionForce(settings->hz));
		}	

		if(!settings->isRecord && (settings->isReplay || settings->enablePerformanceTestWithPositionFeedback || settings->enablePerformanceTestWithVelocityFeedback) && m_replayIterations != 0)
		{
			if(settings->enablePerformanceTestWithPositionFeedback || settings->enablePerformanceTestWithVelocityFeedback)
				settings->enableGeneralAdaptation = 1;

			static int timeStep = 0;

			if(timeStep == 0)
			{
				m_box->SetBoxTouched(false);
				m_externalForceCollection.clear();
				m_fingerPositionCollection.clear();
				m_fingerVelocityCollection.clear();

				if(m_replayIterations == -1)
					m_replayIterations = settings->replayIterations;

				if(m_mouseJoint)
				{
					m_world->DestroyJoint(m_mouseJoint);
					m_mouseJoint = NULL;
				}

				if(m_replayIterations == 5)
				{
					if(m_finger->GetFingerBody())
					{
						float32 fingerFriction = FINGER_FRICTION;
						float32 fingerDensity = FINGER_DENSITY;
						float32 fingerSize = FINGER_SIZE;
						b2Vec2 fingerPosition = FINGER_POSITION;
						delete m_finger;
						m_finger = new Finger(m_world, fingerPosition, fingerSize, fingerDensity, fingerFriction);
					}
				}

				else
				{
					AddNoise(settings);
					if(settings->enableBoxShiftingUpNoise && settings->enableShiftingUpAdaptation)
						m_adaptation = new Adaptation(Adaptation::e_shiftingUpAdaptationBit);
					else if(settings->enableBoxShiftingDownNoise && settings->enableShiftingDownAdaptation)
						m_adaptation = new Adaptation(Adaptation::e_shiftingDownAdaptationBit);
					else if(settings->enableBoxShiftingLeftNoise && settings->enableShiftingLeftAdaptation)
						m_adaptation = new Adaptation(Adaptation::e_shiftingLeftAdaptationBit);
					else if(settings->enableBoxShiftingRightNoise && settings->enableShiftingRightAdaptation)
						m_adaptation = new Adaptation(Adaptation::e_shiftingRightAdaptationBit);
					else if(settings->enableGeneralAdaptation)
						m_adaptation = new Adaptation(Adaptation::e_generalAdaptationBit);
				}
			}

			RecordInfo recordInfo = m_recordInfos.at(timeStep);
			m_finger->SetPullPointPosition(recordInfo.GetPullPointPosition());
			m_flag = recordInfo.GetFlag();

			if(!m_mouseJoint)
				m_finger->SetFingerReactionForce(b2Vec2(0.0f, 0.0f));
			else
				m_finger->SetFingerReactionForce(m_mouseJoint->GetReactionForce(settings->hz));

			if(settings->enableBoxShiftingUpNoise && settings->enableShiftingUpAdaptation
				|| settings->enableBoxShiftingDownNoise && settings->enableShiftingDownAdaptation
				|| settings->enableBoxShiftingLeftNoise && settings->enableShiftingLeftAdaptation
				|| settings->enableBoxShiftingRightNoise && settings->enableShiftingRightAdaptation || settings->enableGeneralAdaptation)
			{
				if(m_adaptation != NULL)
				{
					b2Vec2 pullPointPosition = m_adaptation->MyAdaptation(timeStep, m_box->IsBoxTouched(), m_recordInfos, m_actualInfos, m_phases, m_finger);
					m_finger->SetPullPointPosition(pullPointPosition);
				}
			}

			if(!m_flag.compare("3") || !m_flag.compare("4"))
			{
				if(m_mouseJoint == NULL)
				{
					b2MouseJointDef md;
					b2BodyDef bodyDef;
					b2Body* emptyBody = m_world->CreateBody(&bodyDef);
					md.bodyA = emptyBody;
					md.bodyB = m_finger->GetFingerBody();
					md.target = fingerPosition;
					md.maxForce = 10000.0f * m_finger->GetFingerBody()->GetMass();
					md.dampingRatio = m_mouseJointDampingRatio;
					md.frequencyHz = m_mouseJointFrequency;
					m_mouseWorld = fingerPosition;
					m_mouseJoint = (b2MouseJoint*)m_world->CreateJoint(&md);
					m_finger->GetFingerBody()->SetAwake(true);
				}
				m_mouseWorld = m_finger->GetPullPointPosition();
				m_mouseJoint->SetTarget(m_finger->GetPullPointPosition());
			}

			if(!m_flag.compare("1") || !m_flag.compare("2"))
			{
				if(m_mouseJoint != NULL)
				{
					m_world->DestroyJoint(m_mouseJoint);
					m_mouseJoint = NULL;
				}
			}

			vector<Phase> :: iterator iter;
			int i = 0;
			for(iter = m_phases.begin(); iter < m_phases.end(); iter++, i++)
			{
				if(timeStep >= (*iter).startTimeStep && timeStep < (*iter).endTimeStep)
				{
					m_currentPhase = i;
				}
			}

			timeStep++;

			if(m_box->IsBoxTouched() && m_mouseJoint)
			{
				//m_finger->DrawForce(m_mouseJoint->GetReactionForce(settings->hz), m_debugDraw);
			}

			if(timeStep >= m_recordInfos.size())
			{
				m_replayIterations--;				
				timeStep = 0;
				StopReplay(settings);
			}
		}
		else
		{
			if(m_box->IsBoxTouched() && m_mouseJoint)
			{
				//m_finger->DrawForce(m_mouseJoint->GetReactionForce(settings->hz), m_debugDraw);
			}
			static int timeStep = 0;
			if(settings->isRecord)
			{
				Recording(settings, timeStep++);
			}
			else
			{
				timeStep = 0;
			}
		}

		b2Vec2 invGravityForce = m_world->GetGravity();
		invGravityForce *= -1.0f * m_finger->GetFingerBody()->GetMass();
		m_finger->GetFingerBody()->ApplyForceToCenter(invGravityForce);

		Test::Step(settings);

		b2Vec2 externalForce = m_finger->GetFingerReactionForce();
		externalForce += invGravityForce;
		m_externalForceCollection.push_back(externalForce);
		m_fingerPositionCollection.push_back(m_finger->GetFingerPosition());
		m_fingerVelocityCollection.push_back(m_finger->GetFingerBody()->GetLinearVelocity());
	}

	void DrawInfo(Settings* settings)
	{
		{
			char infoValue[15];
			char title[50] = "BoxSize: ";
			sprintf(infoValue, "%.6f", m_box->GetBoxSize());
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[15];
			char title[30] = "BoxDensity: ";
			sprintf(infoValue, "%.6f", m_box->GetBoxDensity());
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[15];
			char title[50] = "BoxFriction: ";
			sprintf(infoValue, "%.6f", m_box->GetBoxFriction());
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[15];
			char title[50] = "FingerFriction: ";
			sprintf(infoValue, "%.6f", m_finger->GetFingerFriction());
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[15];
			char title[50] = "GroundFriction: ";
			sprintf(infoValue, "%.6f", m_ground->GetGroundFriction());
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[15];
			char title[50] = "MouseJointFrequency: ";
			sprintf(infoValue, "%.6f", m_mouseJointFrequency);
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[15];
			char title[50] = "MouseJointDampingRatio: ";
			sprintf(infoValue, "%.6f", m_mouseJointDampingRatio);
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[20];
			char title[50] = "GroundBodyPosition: ";
			sprintf(infoValue, "%.6f %.6f", m_ground->GetGroundPosition().x, m_ground->GetGroundPosition().y);
			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[100];
			char title[150] = "FeedBack: ";
			sprintf(infoValue, "%.2f %.2f %.2f %.2f %.2f %.2f", 
				Adaptation::m_fbArray[0][0], 
				Adaptation::m_fbArray[0][1], 
				Adaptation::m_fbArray[0][2], 
				Adaptation::m_fbArray[0][3], 
				Adaptation::m_fbArray[0][4], 
				Adaptation::m_fbArray[0][5]
			);

			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		{
			char infoValue[100];
			char title[150] = "          ";
			sprintf(infoValue, "%.2f %.2f %.2f %.2f %.2f %.2f", 
				Adaptation::m_fbArray[1][0], 
				Adaptation::m_fbArray[1][1], 
				Adaptation::m_fbArray[1][2], 
				Adaptation::m_fbArray[1][3], 
				Adaptation::m_fbArray[1][4], 
				Adaptation::m_fbArray[1][5]
			);

			m_debugDraw.DrawString(5, m_textLine, strcat(title, infoValue));
			m_textLine += 15;
		}

		if (settings->pause)
		{
			m_debugDraw.DrawString(5, m_textLine, "****PAUSED****");
			m_textLine += 15;
		}

		if (settings->isRecord)
		{
			m_debugDraw.DrawString(5, m_textLine, "****Recording****");
			m_textLine += 15;
		}

		if (settings->isReplay)
		{
			m_debugDraw.DrawString(5, m_textLine, "****Replay****");
			m_textLine += 15;
		}
	}

	void DrawPhases(Settings* settings)
	{
		if (settings->isReplay)
		{
			for(int i = 0; i < m_phases.size(); i++)
			{
				b2Color color;
				if(m_currentPhase == i)
					color = b2Color(1.0f, 0.1f, 0.1f);
				else
					color = b2Color(0.5f, 0.5f, 0.5f);

				m_debugDraw.DrawCircle(b2Vec2(2.5f + 0.4f * i, 2.7f), 0.1f, color);
			}

			m_debugDraw.DrawString(1115, 100, "Approach");
			m_debugDraw.DrawString(1210, 100, "Tip");
			m_debugDraw.DrawString(1280, 100, "Push");
		}
	}

	string CurrentDateTime() 
	{
		time_t now = time(0);
		struct tm  tstruct;
		char buf[80];
		tstruct = *localtime(&now);
		strftime(buf, sizeof(buf), "%Y-%m-%d-%H-%M-%S", &tstruct);
		return buf;
	}

	void Recording(Settings* settings, int timeStep)
	{
		if(m_fileName.empty())
		{
			m_fileName = CurrentDateTime();
			settings->fileName = m_fileName;
		}
		if(!m_trajectoryOutput)
		{
			char path[100] = ".\\trajectory\\";
			strcat(path, m_fileName.c_str());
			m_trajectoryOutput = fopen(path, "at+");
			m_recordInfos.clear();
		}

		//static int timeStep = 0;
		b2Vec2 fingerPosition = m_finger->GetFingerPosition();
		b2Vec2 pullPointPosition = m_finger->GetPullPointPosition();
		b2Vec2 fingerVelocity = m_finger->GetFingerVelocity();
		b2Vec2 fingerForce = m_finger->GetFingerReactionForce();
		//b2Vec2 fingerForce;
		//if(m_mouseJoint)
		//	fingerForce = m_mouseJoint->GetReactionForce(settings->hz);
		//else
		//	fingerForce.SetZero();

		m_phases.clear();

		if(m_mouseJoint == NULL && !m_box->IsBoxTouched())
		{
			RecordInfo recordInfo(fingerPosition, pullPointPosition, fingerVelocity, fingerForce, "1", timeStep);
			m_recordInfos.push_back(recordInfo);
		}
		else if(m_mouseJoint == NULL && m_box->IsBoxTouched())
		{
			RecordInfo recordInfo(fingerPosition, pullPointPosition, fingerVelocity, fingerForce, "2", timeStep);
			m_recordInfos.push_back(recordInfo);
		}
		else if(m_mouseJoint != NULL && m_box->IsBoxTouched())
		{
			RecordInfo recordInfo(fingerPosition, pullPointPosition, fingerVelocity, fingerForce, "3", timeStep);
			m_recordInfos.push_back(recordInfo);
		}
		else if(m_mouseJoint != NULL && !m_box->IsBoxTouched())
		{
			RecordInfo recordInfo(fingerPosition, pullPointPosition, fingerVelocity, fingerForce, "4", timeStep);
			m_recordInfos.push_back(recordInfo);
		}
	}

	void StopRecord()
	{
		if(m_trajectoryOutput)
		{
			vector<RecordInfo> :: iterator iter;
			for(iter = m_recordInfos.begin(); iter < m_recordInfos.end(); iter++)
				fprintf(m_trajectoryOutput, "%d %s %f %f %f %f %f %f %f %f\n", 
				(*iter).GetTimeStep(), (*iter).GetFlag().c_str(), 
				(*iter).GetFingerPosition().x, (*iter).GetFingerPosition().y,
				(*iter).GetPullPointPosition().x, (*iter).GetPullPointPosition().y,
				(*iter).GetFingerVelocity().x, (*iter).GetFingerVelocity().y,
				(*iter).GetFingerForce().x, (*iter).GetFingerForce().y
				);
			fclose(m_trajectoryOutput);
			m_trajectoryOutput = NULL;
		}
		m_fileName = "";
		PhaseParser();
	}

	void AddNoise(Settings* settings)
	{
		static float32 boxFriction = BOX_FRICTION;
		static float32 groundFriction = GROUND_FRICTION;
		static float32 fingerFriction = FINGER_FRICTION;
		static float32 boxDensity = BOX_DENSITY;
		static float32 boxSize = BOX_SIZE;
		static float32 fingerDensity = FINGER_DENSITY;
		static float32 fingerSize = FINGER_SIZE;
		static b2Vec2 groundPosition = GROUND_POSITION;
		static b2Vec2 fingerPosition = FINGER_POSITION;

		if(m_noise)
		{
			delete m_noise;
			m_noise = NULL;
		}

		m_actualInfos.clear();

		if(m_finger->GetFingerBody())
		{
			delete m_finger;
			m_finger = new Finger(m_world, fingerPosition, fingerSize, fingerDensity, fingerFriction);
		}
		if(m_noiseFlag & e_boxDensityBit)
		{
			b2Vec2 boxPosition = m_box->GetBoxPosition();
			boxDensity += 350.0f;
			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize, groundPosition.y + boxSize), boxSize, boxDensity, boxFriction);
			m_noise = new Noise(m_noiseFlag, "density", boxDensity);
		}
		if(m_noiseFlag & e_boxSizeBit)
		{
			b2Vec2 boxPosition = m_box->GetBoxPosition();
			boxSize += 0.01f;
			boxPosition.x += 0.01f;
			boxPosition.y += 0.01f;
			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize, groundPosition.y + boxSize), boxSize, boxDensity, boxFriction);
			m_noise = new Noise(m_noiseFlag, "size", boxSize);
		}
		if(m_noiseFlag & e_boxFrictionBit)
		{
			boxFriction += 0.1f;
			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize, groundPosition.y + boxSize), boxSize, boxDensity, boxFriction);
			m_noise = new Noise(m_noiseFlag, "box_friction", boxFriction);
		}
		if(m_noiseFlag & e_fingerFrictionBit)
		{
			fingerFriction += 0.05f;
			delete m_finger;
			m_finger = new Finger(m_world, fingerPosition, fingerSize, fingerDensity, fingerFriction);
			m_noise = new Noise(m_noiseFlag, "finger_friction", fingerFriction);
		}
		if(m_noiseFlag & e_groundFrictionBit)
		{
			groundFriction += 0.05f;
			delete m_ground;
			m_ground = new Ground(m_world, groundPosition, groundFriction);
			m_noise = new Noise(m_noiseFlag, "ground_friction", groundFriction);
		}
		if(m_noiseFlag & e_frequencyBit)
		{
			m_mouseJointFrequency += 0.5f;
			if(m_mouseJoint)
			{
				m_world->DestroyJoint(m_mouseJoint);
				m_mouseJoint = NULL;
			}
			m_noise = new Noise(m_noiseFlag, "spring_frequency", m_mouseJointFrequency);
		}
		if(m_noiseFlag & e_dampingRatioBit)
		{
			m_mouseJointDampingRatio += 0.05f;
			if(m_mouseJoint)
			{
				m_world->DestroyJoint(m_mouseJoint);
				m_mouseJoint = NULL;
			}
			m_noise = new Noise(m_noiseFlag, "spring_damping_ratio", m_mouseJointDampingRatio);
		}
		if(m_noiseFlag & e_boxShiftingUpBit)
		{
			groundPosition.y += 0.1f;

			delete m_ground;
			m_ground = new Ground(m_world, groundPosition, groundFriction);

			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize, groundPosition.y + boxSize), boxSize, boxDensity, boxFriction);
			m_noise = new Noise(m_noiseFlag, "box_up", groundPosition.y);
		}
		if(m_noiseFlag & e_boxShiftingDownBit)
		{
			groundPosition.y -= 0.1f;

			delete m_ground;
			m_ground = new Ground(m_world, groundPosition, groundFriction);

			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize, groundPosition.y + boxSize), boxSize, boxDensity, boxFriction);
			m_noise = new Noise(m_noiseFlag, "box_down", groundPosition.y);
		}

		if(m_noiseFlag & e_boxShiftingLeftBit)
		{
			groundPosition.x -= 0.05f;

			delete m_ground;
			m_ground = new Ground(m_world, groundPosition, groundFriction);

			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize + groundPosition.x, boxSize), boxSize, boxDensity, boxFriction);
			m_noise = new Noise(m_noiseFlag, "box_left", groundPosition.x);
		}

		if(m_noiseFlag & e_boxShiftingRightBit)
		{
			groundPosition.x += 0.05f;

			delete m_ground;
			m_ground = new Ground(m_world, groundPosition, groundFriction);

			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize + groundPosition.x, boxSize), boxSize, boxDensity, boxFriction);
			m_noise = new Noise(m_noiseFlag, "box_right", groundPosition.x);
		}

		if(m_replayIterations == 1)
		{
			boxFriction = BOX_FRICTION;
			groundFriction = GROUND_FRICTION;
			fingerFriction = FINGER_FRICTION;
			boxDensity = BOX_DENSITY;
			boxSize = BOX_SIZE;
			fingerDensity = FINGER_DENSITY;
			fingerSize = FINGER_SIZE;
			groundPosition = GROUND_POSITION;
			fingerPosition = FINGER_POSITION;
		}
	}

	void StopReplay(Settings* settings)
	{
		static int iterNum = 1;
		static int fileCount = 1;

		if(settings->enableBoxShiftingUpNoise && settings->enableShiftingUpAdaptation
			|| settings->enableBoxShiftingDownNoise && settings->enableShiftingDownAdaptation
			|| settings->enableBoxShiftingLeftNoise && settings->enableShiftingLeftAdaptation
			|| settings->enableBoxShiftingRightNoise && settings->enableShiftingRightAdaptation || settings->enableGeneralAdaptation)
		{
			delete m_adaptation;
			m_adaptation = NULL;

			if((settings->enablePerformanceTestWithPositionFeedback || settings->enablePerformanceTestWithVelocityFeedback) && m_noise != NULL)
			{
				string fileName = settings->fileName;
				char path[200] = ".\\stats\\";
				string p = settings->enablePerformanceTestWithPositionFeedback ? "-p" : "";
				string v = settings->enablePerformanceTestWithVelocityFeedback ? "-v" : "";

				std::ostringstream oss;

				oss << fileName << "-" << m_noise->getNoiseType() << "-" << fileCount << "-" << iterNum++ << p << v;
				strcat(path, oss.str().c_str());
				m_actualFile = fopen(path, "at+");

				fprintf(m_actualFile, "%s %.2f\n", m_noise->getNoiseType().c_str(), m_noise->getNoiseMagnitude());
				fprintf(m_actualFile, "%.2f %.2f %.2f %.2f %.2f %.2f\n", 
					Adaptation::m_fbArray[0][0], Adaptation::m_fbArray[0][1], Adaptation::m_fbArray[0][2], Adaptation::m_fbArray[0][3], Adaptation::m_fbArray[0][4], Adaptation::m_fbArray[0][5]);
				fprintf(m_actualFile, "%.2f %.2f %.2f %.2f %.2f %.2f\n", 
					Adaptation::m_fbArray[1][0], Adaptation::m_fbArray[1][1], Adaptation::m_fbArray[1][2], Adaptation::m_fbArray[1][3], Adaptation::m_fbArray[1][4], Adaptation::m_fbArray[1][5]);


				vector<ActualInfo> :: iterator iter;
				for(iter = m_actualInfos.begin(); iter < m_actualInfos.end(); iter++)
					fprintf(m_actualFile, " %d %s %d %d %s %f %f %f %f %f %f %f %f\n", 
					(*iter).GetPhase(), BoolToString((*iter).GetExtended()), 
					(*iter).GetTimeStep(), (*iter).GetNewTimeStep(), (*iter).GetFlag().c_str(),
					(*iter).GetFingerPosition().x, (*iter).GetFingerPosition().y,
					(*iter).GetPullPointPosition().x, (*iter).GetPullPointPosition().y,
					(*iter).GetFingerVelocity().x, (*iter).GetFingerVelocity().y,
					(*iter).GetFingerForce().x, (*iter).GetFingerForce().y,
					(*iter).GetPhase(), BoolToString((*iter).GetExtended())
					);
				fclose(m_actualFile);
				m_actualFile = NULL;
			}
		}

		else
		{
			char iterStr[4];
			itoa(iterNum, iterStr, 10);

			if(m_dataFile == NULL)
			{
				m_dataFile = fopen(".\\data_file_name", "w");
			}
			fprintf(m_dataFile, "%s-%d\n", settings->fileName.c_str(), iterNum);

			uint32 flags = 0;
			flags += settings->enableForceDataWrite * e_writeForce;
			flags += settings->enablePositionDataWrite * e_writePosition;
			flags += settings->enableVelocityDataWrite * e_writeVelocity;

			if(flags & e_writeForce)
			{	
				char path[100] = ".\\figure_data\\force\\";
				strcat(strcat(strcat(path, settings->fileName.c_str()),"-"), iterStr);
				FILE* forceDataOutput = fopen(path, "w");
				vector<b2Vec2>::iterator iter;
				for(iter = m_externalForceCollection.begin(); iter != m_externalForceCollection.end(); iter++) 
					fprintf(forceDataOutput, "%f %f\n", (*iter).x, (*iter).y);
				fclose(forceDataOutput);
			}

			if(flags & e_writePosition)
			{
				char path[100] = ".\\figure_data\\position\\";
				strcat(strcat(strcat(path, settings->fileName.c_str()),"-"), iterStr);
				FILE* positionDataOutput = fopen(path, "w");
				vector<b2Vec2>::iterator iter;
				for(iter = m_fingerPositionCollection.begin(); iter != m_fingerPositionCollection.end(); iter++) 
					fprintf(positionDataOutput, "%f %f\n", (*iter).x, (*iter).y);
				fclose(positionDataOutput);
			}

			if(flags & e_writeVelocity)
			{
				char path[100] = ".\\figure_data\\velocity\\";
				strcat(strcat(strcat(path, settings->fileName.c_str()),"-"), iterStr);
				FILE* velocityDataOutput = fopen(path, "w");
				vector<b2Vec2>::iterator iter;
				for(iter = m_fingerVelocityCollection.begin(); iter != m_fingerVelocityCollection.end(); iter++) 
					fprintf(velocityDataOutput, "%f %f\n", (*iter).x, (*iter).y);
				fclose(velocityDataOutput);
			}
			iterNum++;
			fclose(m_dataFile);
			m_dataFile = NULL;
		}

		if(m_replayIterations == 0)
		{
			settings->isReplay = !settings->isReplay;

			float32 boxFriction = BOX_FRICTION;
			float32 groundFriction = GROUND_FRICTION;
			float32 fingerFriction = FINGER_FRICTION;
			float32 boxDensity = BOX_DENSITY;
			float32 boxSize = BOX_SIZE;
			float32 fingerDensity = FINGER_DENSITY;
			float32 fingerSize = FINGER_SIZE;
			b2Vec2 groundPosition = GROUND_POSITION;
			b2Vec2 fingerPosition = FINGER_POSITION;

			delete m_box;
			m_box = new Box(m_world, b2Vec2(-1.5f + boxSize, groundPosition.y + boxSize), boxSize, boxDensity, boxFriction);

			delete m_ground;
			m_ground = new Ground(m_world, groundPosition, groundFriction);

			delete m_finger;
			m_finger = new Finger(m_world, fingerPosition, fingerSize, fingerDensity, fingerFriction);

			m_replayIterations = -1;
			
			iterNum = 1;
			fileCount++;

			if(settings->enablePerformanceTestWithPositionFeedback)
			{
				Adaptation::m_fbArray[0][0] += 0.05;
				Adaptation::m_fbArray[1][1] += 0.05;
			}

			if(settings->enablePerformanceTestWithVelocityFeedback)
			{
				Adaptation::m_fbArray[0][2] -= 0.01;
				Adaptation::m_fbArray[1][3] -= 0.01;
			}
			if(m_noise)
			{
				delete m_noise;
				m_noise = NULL;
			}
		}

		if(m_trajectoryInput)
		{		
			fclose(m_trajectoryInput);
			m_trajectoryInput = NULL;
		}

		if(m_mouseJoint)
		{
			m_world->DestroyJoint(m_mouseJoint);
			m_mouseJoint = NULL;
		}
	}

	void PhaseParser()
	{
		std::vector<RecordInfo>::iterator iter_2 = std::find_if(m_recordInfos.begin(), m_recordInfos.end(), Adaptation::RecordInfo_flag_equ("2"));
		int timeStep_2 = iter_2 == m_recordInfos.end() ? m_recordInfos.size() : iter_2 -> GetTimeStep();
		std::vector<RecordInfo>::iterator iter_3 = std::find_if(m_recordInfos.begin(), m_recordInfos.end(), Adaptation::RecordInfo_flag_equ("3"));
		int timeStep_3 = iter_3 == m_recordInfos.end() ? m_recordInfos.size() : iter_3 -> GetTimeStep();

		int separator_1_2 = timeStep_2 < timeStep_3 ? timeStep_2 : timeStep_3;

		int separator_2_3 = (*max_element(m_recordInfos.begin() + separator_1_2, m_recordInfos.end(), 
			[](RecordInfo a, RecordInfo b)
		{
			return a.GetFingerPosition().x < b.GetFingerPosition().x;
		})
			).timeStep;

		Phase phase_1(m_recordInfos, 0, separator_1_2, "approach", Phase::e_approachPhase);
		m_phases.push_back(phase_1);

		Phase phase_2(m_recordInfos, separator_1_2, separator_2_3, "tip", Phase::e_tipPhase);
		m_phases.push_back(phase_2);

		Phase phase_3(m_recordInfos, separator_2_3, m_recordInfos.size(), "push back", Phase::e_pushbackPhase);
		m_phases.push_back(phase_3);
	}

	inline const char * const BoolToString(bool b)
	{
		return b ? "true" : "false";
	}

	Finger* m_finger;
	Box* m_box;
	Ground* m_ground;

	FILE* m_trajectoryInput;
	FILE* m_trajectoryOutput;
	FILE* m_dataFile;
	FILE* m_actualFile;
	string m_fileName;
	string m_flag;
	Noise* m_noise;
	uint32 m_noiseFlag;
	int32 m_replayIterations;
	int m_currentPhase;

	float32 m_mouseJointFrequency;
	float32 m_mouseJointDampingRatio;

	Adaptation* m_adaptation;

	std::vector<b2Vec2> m_externalForceCollection;
	std::vector<b2Vec2> m_fingerPositionCollection;
	std::vector<b2Vec2> m_fingerVelocityCollection;
	std::vector<RecordInfo> m_recordInfos;
	std::vector<ActualInfo> m_actualInfos;
	std::vector<Phase> m_phases;
};

#endif

